#!/usr/bin/env python
#coding:utf-8
import re
import tf
import json
import time
import math
import rospy
from socket import *
from windows_communicator.msg import MzNavState
from std_msgs.msg import Float32MultiArray

SERVER_ADDR = ("10.0.0.104", 8899)
LOOP_RATE = 10  # [HZ]
StateDict = {"STOP":0, "MOVE_WITH_FACE_DETECT":1, "MOVE_WITHOUT_FACE_DETECT":2}

class Client():
    def __init__(self):
        self.__face_ratio = 0        # 目标人物人脸在视野内占比
        self.voltage1 = self.voltage2 = 29.4
        self.listener = tf.TransformListener()
        self.pubber = rospy.Publisher('nav_state', MzNavState, queue_size=10)
        rospy.Subscriber('voltages', Float32MultiArray, self.VoltageCb)

        self.Connect()

        rate = rospy.Rate(LOOP_RATE)
        count = 0
        while not rospy.is_shutdown():
            sendData = self.pack("state")
            try:
                self.tcpClientSocket.send(sendData.encode('utf-8'))
                try:
                    recv = self.tcpClientSocket.recv(1024)
                    recv = self.unpack(recv.decode('utf-8'))
                    json_data = json.loads(recv)
                    working_state = StateDict[json_data['working_state']]
                    pos_x = float(json_data['target_pos']['pos_x'])
                    pos_y = float(json_data['target_pos']['pos_y'])
                    pos_theta = float(json_data['target_pos']['pos_theta'])
                    if(count == LOOP_RATE):  # 1HZ
                        # self.UpdateFaceState()
                        self.SendBatteryState()
                        self.SendPosState()
                        count = 0
                    pub_data = MzNavState(working_state, pos_x, pos_y, pos_theta, self.__face_ratio)
                    self.pubber.publish(pub_data)
                    count += 1
                except Exception as e:
                    rospy.logwarn('[Exception in handling TCP msg]: {}'.format(e))
            except:
                self.Connect()
            rate.sleep()

    def Connect(self):
        self.tcpClientSocket = socket(AF_INET, SOCK_STREAM)
        not_connected = 1
        while not_connected:
            try:
                self.tcpClientSocket.connect(SERVER_ADDR)
                not_connected = 0
            except:
                print("Connecting failed, keep trying......")
            time.sleep(1)
        print("Connected!")

    def UpdateFaceState(self):
        '''低频更新目标人脸检测结果'''
        sendData = self.pack("face")
        self.tcpClientSocket.send(sendData.encode('utf-8'))
        recv = self.tcpClientSocket.recv(1024)
        recv = self.unpack(recv.decode('utf-8'))
        json_data = json.loads(recv)
        top = int(json_data['facedata_b1']['rect']['top'])
        bottom = int(json_data['facedata_b1']['rect']['bottom'])
        left = int(json_data['facedata_b1']['rect']['left'])
        right = int(json_data['facedata_b1']['rect']['right'])
        rect = (bottom-top)*(right-left)
        self.__face_ratio = rect/(1920*1080)

    MAX_V = 28.6  # 此处稍微缩窄，更符合0-100%
    MIN_V = 25.2
    def SendBatteryState(self):
        '''发送电池电量 29.4V - 25.2V(3.6*7)直接线性映射'''
        v1 = self.MAX_V if self.voltage1 > self.MAX_V else self.voltage1
        v2 = self.MAX_V if self.voltage2 > self.MAX_V else self.voltage2
        v1 = self.MIN_V if self.voltage1 < self.MIN_V else self.voltage1
        v2 = self.MIN_V if self.voltage2 < self.MIN_V else self.voltage2
        v1_percent = 100.0*(v1 - self.MIN_V)/(self.MAX_V - self.MIN_V)
        v2_percent = 100.0*(v2 - self.MIN_V)/(self.MAX_V - self.MIN_V)
        sendStr = "battery:{:.2f}%{:.2f}%".format(v1_percent, v2_percent)
        sendData = self.pack(sendStr)
        self.tcpClientSocket.send(sendData.encode('utf-8'))

    def VoltageCb(self, msg):
        # 传到上位机的是0-3.3V的采样电压,经过实测换算比例并非29.4/3.3，而是大概为9.9
        self.voltage1 = msg.data[0]*9.9
        self.voltage2 = msg.data[1]*9.9

    def SendPosState(self):
        '''发送当前位姿，x,y,theta，用于判断是否到达终点'''
        now = rospy.Time.now()
        self.listener.waitForTransform("/map", "/base_link", now, rospy.Duration(1.0))
        t, q = self.listener.lookupTransform("/map", "base_link", now)
        x,y,z,w = q
        yaw = math.atan2(2*(w*z+x*y), 1-2*(z*z+y*y))
        X,Y,theta = t[0], t[1], yaw
        sendStr = "pos:{:.2f} {:.2f} {:.2f}".format(X, Y, theta)
        sendData = self.pack(sendStr)
        self.tcpClientSocket.send(sendData.encode('utf-8'))

    def unpack(self, recv):
        obj = re.match(r"({.*})\r\n\r\n", recv)
        head_len = len(obj.group(1))
        head = json.loads(obj.group(1))
        length = head['len']
        json_str = recv[head_len+4 : head_len+4+length]
        return json_str
    
    def pack(self, send_str):
        head = {"s":"Normal", "len":len(send_str)}
        return json.dumps(head) + '\r\n\r\n' + send_str


if __name__ == '__main__':
    rospy.init_node('windows_client', anonymous=True)
    Client()
